#!/bin/bash

echo "Starting can..."
sudo modprobe gs_usb 
sudo ip link set can0 up type can bitrate 500000
echo "---------------------------can started---------------------------"
sleep 2
# 启动 roscore
echo "Starting roscore..."
roscore &
ROSCORE_PID=$!
echo "---------------------------roscore started---------------------------"
sleep 5

# 启动激光雷达
echo "Starting LiDAR..."
roslaunch scout_bringup open_rslidar.launch > /dev/null 2>&1 &
LIDAR_PID=$!
sleep 5

# 启动gmapping
echo "Starting gmapping..."
roslaunch scout_bringup gmapping.launch &
GMAPPING_PID=$!
sleep 5

echo "---------------------------car's started---------------------------"

# 定义停止所有进程的函数
cleanup() {
    echo -e '\n---------------------------Stopping---------------------------\n'
    kill -TERM $ROSCORE_PID $LIDAR_PID $GMAPPING_PID
    wait $ROSCORE_PID $LIDAR_PID $GMAPPING_PID
    echo -e '\n---------------------------All processes stopped---------------------------\n'
    exit
}

# 捕捉 Ctrl+C 信号以便停止所有进程
trap cleanup SIGINT

# 等待用户输入 's' 来保存地图
while true; do
    read -n 1 -s -r -p "Press 's' to save the map: " key
    if [[ $key == 's' ]]; then
        echo -e '\n---------------------------Saving map...---------------------------\n'
        rosrun map_server map_saver -f ~/catkin_ws/src/scout_base/scout_description/maps/map
        echo -e '\n---------------------------Map saved---------------------------\n'
    fi
done

# 保持脚本运行
while true; do
    sleep 1
done